int _target_motor_step = 0;
int _target_motor_speed = 0;
boolean _target_motor_move_left = 0;
boolean _target_motor_move_right = 0;

//////////////////////////////
// target
void target_stop(void)
{
  _target_mode_enable = false;
  _target_type = _target_type_none;
  motor_stop();
}
void move_to_target(const boolean f, const char spd, const int len)
{
  _target_mode_enable = false;
  reinit_encoders();
  _target_type = _target_type_move;
  _target_step = len;
  _target_motor_speed = spd;
  _target_motor_move_left = _target_motor_move_right = f;
  _target_motor_step = 0;
  _target_mode_enable = true;  
}
void rotate_to_target(const char spd, const int bearing)
{
  _target_mode_enable = false;
  reinit_encoders();
  _get_body_compass();
  _target_motor_step = abs(bearing-body_compass_bearing);
  _target_type = _target_type_rotate;
  _target_step = bearing;
  _target_motor_speed = spd;
  _target_mode_enable = true;  
}

void _set_target_turn(int current, int target)
{
  int dist=(target-current)%360;
  dist=dist<0?dist+360:dist;
  if(dist>=180)
  {
    _target_motor_move_left=1;
    _target_motor_move_right=0;
  }
  else
  {
    _target_motor_move_left=0;
    _target_motor_move_right=1;
  }
}

void check_reach_target()
{
  if(!_target_mode_enable) return;

  switch(_target_type)
  {
  case _target_type_move:
    {
      //long current_step = int((_encoder_step_left+_encoder_step_right)/2);
      long current_step = int(_encoder_step_left);

      if(_target_motor_step==0
        || (_target_motor_step==4 && current_step>=(_target_step*0.95f)))
      {
        int speedstep = _target_motor_speed/4;
        speedstep = constrain(speedstep, 5, 100);
        motor_action(map_power_motor(speedstep),  map_power_motor(speedstep), _target_motor_move_left, _target_motor_move_right);
        _target_motor_step++;
      }
      else if((_target_motor_step==1 && current_step>(_target_step*0.05f))
        || (_target_motor_step==3 && current_step>(_target_step*0.85f)))
      {
        int speedstep = _target_motor_speed/2;
        speedstep = constrain(speedstep, 10, 100);
        motor_action(map_power_motor(speedstep),  map_power_motor(speedstep), _target_motor_move_left, _target_motor_move_right);
        _target_motor_step++;
      }
      else if(_target_motor_step==2 && current_step>(_target_step*0.15))
      {
        int speedstep = _target_motor_speed;
        speedstep = constrain(speedstep, 15, 100);
        motor_action(map_power_motor(speedstep),  map_power_motor(speedstep), _target_motor_move_left, _target_motor_move_right);
        _target_motor_step++;
      }
      else if(current_step>=_target_step)
      {
        target_stop();
        send_target_ok(-1);
      }
    }
    break;
  case _target_type_rotate:
    {
      long current_step = int(body_compass_bearing);

      int curr_diff = current_step-_target_step;
      curr_diff = abs(curr_diff);
      if(curr_diff==0)
      {
        target_stop();
        send_target_ok(-1);
      }
      else if(curr_diff<=(_target_motor_step*0.15))
      {
        int speedstep = _target_motor_speed/4;
        speedstep = constrain(speedstep, 25, 100);
        _set_target_turn(current_step, _target_step);
        motor_action(map_power_motor(speedstep),  map_power_motor(speedstep), _target_motor_move_left, _target_motor_move_right);
      }
      else if(curr_diff<=(_target_motor_step*0.40))
      {
        int speedstep = _target_motor_speed/2;
        speedstep = constrain(speedstep, 35, 100);
        _set_target_turn(current_step, _target_step);
        motor_action(map_power_motor(speedstep),  map_power_motor(speedstep), _target_motor_move_left, _target_motor_move_right);
      }
      else// if(curr_diff<=(curr_diff*0.25f))
      {
        int speedstep = _target_motor_speed;
        speedstep = constrain(speedstep, 45, 100);
        _set_target_turn(current_step, _target_step);
        motor_action(map_power_motor(speedstep),  map_power_motor(speedstep), _target_motor_move_left, _target_motor_move_right);
      }
    }
    break;
  case _target_type_none:
  default:
    {
      target_stop();
      send_target_ok(-1);
    }
    break;
  }

  // TODO check if is blocked !
}


